#pragma once

#include <array>
#include <string>
#include <vector>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/features/boundary.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include "Evaluator.h"

namespace ExtractShapes {

using Point_t = pcl::PointXYZ;
using Cloud_t = pcl::PointCloud<Point_t>;
using CloudPtr_t = Cloud_t::Ptr;
using Color_t = std::array<int, 3>;
using Normal_t = pcl::PointCloud<pcl::Normal>;
using NormalPtr_t = Normal_t::Ptr;
using BoundaryPtr_t = pcl::PointCloud<pcl::Boundary>::Ptr;
using CoefficientPtr_t = pcl::ModelCoefficients::Ptr;

//  提取一个平面
// Input:
//      cloud: 点云
//      coefficients: 平面参数 ax + by + cz + d = 0
// Output:
//      平面的点云
std::vector<CloudPtr_t> extractPlane(CloudPtr_t cloud, CoefficientPtr_t coefficients);

//  估计点云法向量
// Input:
//      cloud: 点云
// Output:
//      法向量
NormalPtr_t estimateNormal(CloudPtr_t cloud);

//  提取点云边界
// Input:
//      cloud: 点云
//      normals: 点云的法向
// Output:
//      点云边界
BoundaryPtr_t findBoundaries(CloudPtr_t cloud, NormalPtr_t normals);

//  保存点云
// Input:
//      cloud: 点云
//      filename: 文件名，必须是 "***.pcd"形式，可以带路径
void writeCloud(CloudPtr_t cloud, const std::string& filename);

//  可视化一个点云,直到用'q'退出
// Input:
//      cloud: 点云
//      cloudName: 点云的名称
//      color: 色彩,默认为{255, 255, 255}
// Output:
//      点云边界
void show(CloudPtr_t cloud, const std::string& cloudName, const Color_t& color=Color_t({255, 255, 255}));

// 用于可视化
class Viewer {
public:
    Viewer(const std::string& figName) 
    : viewer_(figName){
        viewer_.setBackgroundColor(0.05, 0.05, 0.05);
        viewer_.createViewPort(0.0, 0.0, 1.0, 1.0, viewPort_);
    }

    //  可视化一个点云,直到用'q'退出
    // Input:
    //      cloud: 点云
    //      cloudName: 点云的名称，应当保证所有名称不同
    //      color: 色彩,默认为{255, 255, 255}
    void addPointCloud(CloudPtr_t cloud, const std::string& cloudName, const Color_t& color=Color_t({255, 255, 255}));
    
    // 绘制点云图像
    void show();
private:
    pcl::visualization::PCLVisualizer viewer_;
    int viewPort_;
};

// 随机提取矩形和圆形
class RectangleCircleExtractor {
public:
    RectangleCircleExtractor(const std::string& filename, size_t numOfShapes, size_t minShapeSize=100) 
    : cloudOriginal(new Cloud_t),
      coefficients(new pcl::ModelCoefficients),
      numOfShapes(numOfShapes),
      minShapeSize(minShapeSize) {
        pcl::PCDReader reader;
        reader.read(filename, *cloudOriginal);
    }

    RectangleCircleExtractor(CloudPtr_t cloud, size_t numOfShapes , size_t minShapeSize=100) 
    : cloudOriginal(cloud),
      coefficients(new pcl::ModelCoefficients),
      numOfShapes(numOfShapes),
      minShapeSize(minShapeSize) { }

    void extractAndEval(evaluation::Evaluator& eval);

private:
    float getprojectedZ(float x, float y);

    CloudPtr_t cloudOriginal;
    pcl::ModelCoefficients::Ptr coefficients;
    size_t numOfShapes;
    size_t minShapeSize;
    std::vector<CloudPtr_t> outputs;
};


};